#coding=utf - 8
import rospy
import math
import time
import USART
import os
import sys
import fcntl
import threading

path = os.path.abspath(".")
sys.path.insert(0,path + "/src/car_pkg/scripts")
#sys.path.insert(0,"/home/sunrise/workspace/yolov5-master/")

sender = USART.SerialSender("/dev/ttyS4", 115200)
print("/dev/ttyS4;115200")
received_array=[1,1,1,1,1,1]
flag_fire1 = 1
flag = 0
biaozhi = 0
center_zuobiao = [0,0]
distance = 0
flag_angle = 0
count = 0 

def car_forward():
  received_array[0]=0xFF
  received_array[1]=0x01
  received_array[2]=0x00
  received_array[3]=0x01
  received_array[4]=0x01
  received_array[5]=0xAA
  sender.air_speed(received_array)


def car_left_turn():
  received_array[0]=0xFF
  received_array[1]=0x01
  received_array[2]=0x01
  received_array[3]=0x02
  received_array[4]=0x03
  received_array[5]=0xAA
  sender.air_speed(received_array)
  
def car_right_turn():
  received_array[0]=0xFF
  received_array[1]=0x01
  received_array[2]=0x01
  received_array[3]=0x02
  received_array[4]=0x04
  received_array[5]=0xAA
  sender.air_speed(received_array)
  
  
  
def left_pin():
  received_array[0]=0xFF
  received_array[1]=0x01
  received_array[2]=0x00
  received_array[3]=0x01
  received_array[4]=0x06
  received_array[5]=0xAA
  sender.air_speed(received_array)
  
def right_pin():
  received_array[0]=0xFF
  received_array[1]=0x01
  received_array[2]=0x00
  received_array[3]=0x01
  received_array[4]=0x07
  received_array[5]=0xAA
  sender.air_speed(received_array)  

def angel_computer():
  received_array[0]=0xFF
  received_array[1]=0x07
  received_array[2]=0x00
  received_array[3]=0x00
  received_array[4]=0x05
  received_array[5]=0xAA
  sender.air_speed(received_array)

def car_stop():
  received_array[0]=0xFF
  received_array[1]=0x01
  received_array[2]=0x00
  received_array[3]=0x00
  received_array[4]=0x05
  received_array[5]=0xAA
  sender.air_speed(received_array)
  
def car_for_distance(distance):
  received_array[2] = int(distance)
  #距离小数值
  received_array[3] = int(((distance % 1) * 10)+2)  
  received_array[0]=0xFF
  received_array[1]=0x02
  received_array[4]=0x01
  received_array[5]=0xAA
  sender.air_speed(received_array)
  distime = distance / 0.1
  time.sleep(distime + 10)


def fire_turn():
  global flag_fire1 
  print('fire')
  flag_fire1 = 2
  received_array[0]=0xFF
  received_array[1]=0x04
  received_array[2]=0x00
  received_array[3]=0x00
  received_array[4]=0x08
  received_array[5]=0xAA
  sender.air_speed(received_array)
    
  
  
def fire_stop():
  global flag_fire1 
  if flag_fire1 == 3:
    flag_fire1 = 1
    received_array[0]=0xFF
    received_array[1]=0x04
    received_array[2]=0x00
    received_array[3]=0x00
    received_array[4]=0x07
    received_array[5]=0xAA
    sender.air_speed(received_array)


def fire_check():
    global center_zuobiao
    global biaozhi
    global distance
    global flag
    global flag_angle
    global count 
    flag_init = 0
    while True:
        try:
           file_path = "/home/orangepi/car_web/src/car_pkg/scripts/zuobiao.txt"
           file = open(file_path, "r+")
           # 尝试获取文件锁，如果文件被其他进程独占写，会阻塞
           fcntl.flock(file.fileno(), fcntl.LOCK_SH | fcntl.LOCK_NB)
           content = file.readline()
           content = content.strip()#删除尾部的\n
           elements = content.split(",")#跟句逗号分隔数据
           if len(content) == 0:
              continue
           else:
               if elements[2] not in ["1", "0"] or len(elements)>5:#判断收到的数据是否异常
                 file.seek(0)
                 file.write("000,000,0,0,0.00")
               elif elements[2] == "1":#第三位数据为1时读取数据进行判断
                 center_zuobiao = [int(elements[0]), int(elements[1])]#读取坐标值
                 if  float(elements[4])>0 and float(elements[4]!=distance):#判断是否需要距离并且距离是否更新
                    if flag_init == 0:
                      distance = float(elements[4])
                      flag =0
                      flag_init =1
                    elif flag_init == 1:
                      if float(elements[4]) > distance : 
                        distance = distance 
                        flag = 1
                        print("1")
                      elif float(elements[4]) < distance :
                        distance = float(elements[4])
                        flag =0
                        print("0")
                        #print(distance)
                 index = content.find(",1,")#获取读取标志位的下标
                 file.seek(index + 1)
                 file.write("0")
                 if (center_zuobiao[0] >= 300 and center_zuobiao[0] <= 340) or count == 2:#判断是否进入目标范围
                    file.seek(index + 2)
                    file.write(",2")
                    if count == 0:
                      count = 1

                 elif (center_zuobiao[0] < 300 or center_zuobiao[0] > 340) and center_zuobiao[0]!=0 and flag_angle == 0:
                    file.seek(index + 2)
                    file.write(",0")
                    if count == 1:
                      count = 0
                                  
           # 释放文件锁
           fcntl.flock(file.fileno(), fcntl.LOCK_UN)
           file.close()
           #print(center_zuobiao)
        except Exception as e:
           print(f"An error occurred: {e}")   

def motion_car():
  global center_zuobiao
  global biaozhi
  global distance
  global flag_fire1 
  global flag
  target_distance = 0.8
  num = 0
  forward_flag = 0
  
  global count
  while True:
    try:  
      if (center_zuobiao[0] >= 300 and center_zuobiao[0] <= 340) or count == 1:#判断是否进入此坐标
        if num == 0 :#判断是否为第一次进入测距模式
           car_stop()#如果坐标对准了，就停止小车
           num = 1
        #判断距离值是否在合理喷射灭火器的范围内
        if distance <= target_distance and distance >0:
          print(distance)
          #在合理范围之内就要停止运动，并进行灭火启动
          car_stop()
          #count = 2
          time.sleep(2)
          if flag_fire1 == 1 :
             #fire_turn()
             time.sleep(2)
          
        elif distance > target_distance and flag ==0 :
          print('gogogo')
          #不在合理范围之内要进行前进
          #car_forward()
          print(distance) 
          car_for_distance(distance-target_distance)
          forward_flag == 1
          count =1;
          flag =1
      elif (center_zuobiao[0] < 300 or center_zuobiao[0] > 340) and center_zuobiao[0]!=0 and flag_fire1!=2 and count == 0:#判断是否需要左右转并且在对准之后不再进入此判断
        #如果坐标值没有对准就要进行判断左转或者右转
        if center_zuobiao[0]>340:
          #right_pin()
          print('right')
          #x=1
          car_right_turn()
          time.sleep(0.005)
          car_stop()
        if center_zuobiao[0]<300:
          #left_pin()
          print('left')
          #x=2
          car_left_turn()
          time.sleep(0.005)
          car_stop()
        num = 0
      
    except Exception as e:
      print(f"An error occured in motion__car thread: {e}")


def file_init():
  file_path = "/home/orangepi/car_web/src/car_pkg/scripts/zuobiao.txt"
  file = open(file_path, "w+")
  # 尝试获取文件锁，如果文件被其他进程独占写，会阻塞
  fcntl.flock(file.fileno(), fcntl.LOCK_SH | fcntl.LOCK_NB)
  file.seek(0)
  file.write("0,0,0,0,0.00")
  # 释放文件锁
  fcntl.flock(file.fileno(), fcntl.LOCK_UN)
  file.close()


def thread_main():
  #初始化文件读取
  file_init()
  #多线程跑火焰检测以及小车的移动
  fire_check_thread=threading.Thread(target=fire_check)
  motion_car_thread=threading.Thread(target=motion_car)
  
  fire_check_thread.start()
  motion_car_thread.start()
  fire_check_thread.join()
  motion_car_thread.join()


if __name__ == "__main__":
  thread_main()
    
    





    


    
    
